import heapq
import math

import plotting, env
import AStar


class Dijkstra(AStar):
    """
    TODO:该方法使用BFS算法来搜索从起始节点到目标节点的路径，并返回找到的路径和访问过的顺序。
    Dijkstra set the cost as the priority
    """

    def searching(self):
        """
        Breadth-first Searching.
        :return: path, visited order
        """

        # 代码将起始节点s_start的父节点设置为自身，将起始节点的g值设置为0，并将目标节点的g值设置为正无穷大
        self.PARENT[self.s_start] = self.s_start
        self.g[self.s_start] = 0
        self.g[self.s_goal] = math.inf
        # 代码使用最小堆（heapq），并将(0, self.s_start)作为元组加入OPEN集合中，其中0表示起始节点的优先级
        heapq.heappush(self.OPEN,
                       (0, self.s_start))

        while self.OPEN:
            _, s = heapq.heappop(self.OPEN)
            self.CLOSED.append(s)

            if s == self.s_goal:
                break

            for s_n in self.get_neighbor(s):
                new_cost = self.g[s] + self.cost(s, s_n)

                if s_n not in self.g:
                    self.g[s_n] = math.inf

                if new_cost < self.g[s_n]:  # conditions for updating Cost
                    self.g[s_n] = new_cost
                    self.PARENT[s_n] = s

                    # best first set the heuristics as the priority
                    heapq.heappush(self.OPEN, (new_cost, s_n))

        return self.extract_path(self.PARENT), self.CLOSED


def main():
    s_start = (5, 5)
    s_goal = (45, 25)

    dijkstra = Dijkstra(s_start, s_goal, 'None')
    plot = plotting.Plotting(s_start, s_goal)

    path, visited = dijkstra.searching()
    plot.animation(path, visited, "Dijkstra's")  # animation generate


if __name__ == '__main__':
    main()
